#ifndef GAMEENGINE_AI_NAVMESH_FUNNEL_HPP
#define GAMEENGINE_AI_NAVMESH_FUNNEL_HPP

#include <deque>



#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif

//=================================================================================

//! Default constructor
template <class Vertex>
FunnelTriangle<Vertex>::FunnelTriangle() : v0_(nil), v1_(nil), v2_(nil) {}

//! Constructor
/*!
 @param v0 The first vertex of the triangle
 @param v1 The second vertex of the triangle
 @param v2 The third vertex of the triangle
 */
template <class Vertex>
FunnelTriangle<Vertex>::FunnelTriangle(const Vertex* v0, const Vertex* v1, const Vertex* v2) : v0_(v0), v1_(v1), v2_(v2) {}

//! Copy constructor
/*!
 @param rhs The triangle to copy
 */
template <class Vertex>
FunnelTriangle<Vertex>::FunnelTriangle(const FunnelTriangle& rhs) : v0_(rhs.v0_), v1_(rhs.v1_), v2_(rhs.v2_) {}

//! Destructor
template <class Vertex>
FunnelTriangle<Vertex>::~FunnelTriangle() {}

//! Assignment operator
/*!
 @param rhs The triangle to copy
 */
template <class Vertex>
FunnelTriangle<Vertex>& FunnelTriangle<Vertex>::operator=(const FunnelTriangle& rhs) {
	if ( &rhs != this ) {
		v0_ = rhs.v0_;
		v1_ = rhs.v1_;
		v2_ = rhs.v2_;
	}
	return *this;
}

//! Accessor to the first vertex (read-only)
/*!
 @return A constant pointer to the first vertex
 */
template <class Vertex>
const Vertex* FunnelTriangle<Vertex>::v0() const {
	return v0_;
}

//! Accessor to the second vertex (read-only)
/*!
 @return A constant pointer to the second vertex
 */
template <class Vertex>
const Vertex* FunnelTriangle<Vertex>::v1() const {
	return v1_;
}

//! Accessor to the third vertex (read-only)
/*!
 @return A constant pointer to the third vertex
 */
template <class Vertex>
const Vertex* FunnelTriangle<Vertex>::v2() const {
	return v2_;
}

//=================================================================================

//! Find the shortest path in the given funnel from the start to the end node
/*!
 @param trgl_funnel The funnel, defined as a succession of connected triangles. Triangles
                    are expected to be given in the correct order, i.e., the first triangle
					contains the starting point, the last triangle contains the last point,
					and the n-th triangle is connected to the (n-1)-th and the (n+1)-th
					triangle.
 */
template <class Vertex>
std::list<Vertex> Funnel<Vertex>::find_path(const std::vector<Trgl>& trgl_funnel) {
	// Create an empty path
	std::list<Vertex> path;

	// Check empty funnels, or funnels with one single triangle
	unsigned int sz = (unsigned int)trgl_funnel.size();
	if ( sz <= 1 ) {
		return path;
	}

	// Get start and end points
	const Vertex* start = opposite_vertex(trgl_funnel[0], trgl_funnel[1]);
	if ( start == nil ) return path;
	const Vertex* end = opposite_vertex(trgl_funnel[sz-1], trgl_funnel[sz-2]);
	if ( end == nil ) return path;
	path.push_back(*start);

	// Create a list of the points to be processed
	std::list< FunnelPoint > pts;
	if ( build_funnel(pts, start, end, trgl_funnel) == false ) return path;

	// Build the shortest path
	build_path(path, pts, end);
	return path;
}

//! Find the shortest path in the given funnel from the start to the end node
/*!
 @param trgl_funnel The funnel, defined as a succession of connected triangles. Triangles
                    are expected to be given in the correct order, i.e., the first triangle
					contains the starting point, the last triangle contains the last point,
					and the n-th triangle is connected to the (n-1)-th and the (n+1)-th
					triangle.
 @param radius The radius of the entity trying to go through the funnel. The path should
               therefore never be closer than radius units of distance from the borders of
			   the funnel (except for the start and end points).
 */
template <class Vertex>
std::list<Vertex> Funnel<Vertex>::find_path(const std::vector<Trgl>& trgl_funnel, float radius) {
	// Create an empty path
	std::list<Vertex> path;

	// Check empty funnels, or funnels with one single triangle
	unsigned int sz = (unsigned int)trgl_funnel.size();
	if ( sz <= 1 ) {
		return path;
	}

	// Get start and end points
	const Vertex* start = opposite_vertex(trgl_funnel[0], trgl_funnel[1]);
	if ( start == nil ) return path;
	const Vertex* end = opposite_vertex(trgl_funnel[sz-1], trgl_funnel[sz-2]);
	if ( end == nil ) return path;
	path.push_back(*start);

	// Create a list of the points to be processed
	std::list< FunnelPoint > pts;
	if ( build_funnel(pts, radius, start, end, trgl_funnel) == false ) return path;

	// Build the shortest path
	build_path(path, pts, end);
	return path;
}

//! Build the points of the funnel
/*!
 @param pts Receives the points of the funnel, given with their orientation in the funnel
 @param start The starting point of the path
 @param end The ending point of the path
 @param trgl_funnel The triangles the define the channel
 @return True if a path could be found, false otherwise
 */
template <class Vertex>
bool Funnel<Vertex>::build_funnel(std::list< FunnelPoint >& pts,
								  const Vertex* start, const Vertex* end, const std::vector<Trgl>& trgl_funnel)
{
	// Process first triangle
	pts.push_back( FunnelPoint(*start,NoSide) );

	// Get first interior edge
	InternalEdge edge;
	if ( common_vertices(edge, trgl_funnel[0], trgl_funnel[1]) == false ) return false; // Non-connected triangles

	// Get points order
	const Vertex* v0 = start;
	const Vertex* v1 = edge.first;
	const Vertex* v2 = edge.second;
	const Vertex* right = nil;
	const Vertex* left = nil;

	// Test orientation
	if ( side(*v0, *v1, *v2) == RightSide ) {
		right = v1;
		left = v2;
		pts.push_back( FunnelPoint(*v1,RightSide) );
		pts.push_back( FunnelPoint(*v2,LeftSide) );
	}
	else {
		left = v1;
		right = v2;
		pts.push_back( FunnelPoint(*v1,LeftSide) );
		pts.push_back( FunnelPoint(*v2,RightSide) );
	}

	// Process other points
	for ( unsigned int k = 1; k < (unsigned int)trgl_funnel.size(); ++k ) {
		// Get previous point orientation
		if ( k > 1 ) {
			if ( common_vertices(edge, trgl_funnel[k], trgl_funnel[k-1]) == false ) return false; // Non-connected triangles
			if ( ( right == edge.first ) || ( right == edge.second ) ) {
				pts.rbegin()->second = LeftSide;
				left = v2;
			}
			else {
				pts.rbegin()->second = RightSide;
				right = v2;
			}
		}

		// Add point
		v0 = v1; v1 = v2; v2 = opposite_vertex(trgl_funnel[k], trgl_funnel[k-1]);
		if ( v2 == nil ) return false;
		pts.push_back( FunnelPoint(*v2,NoSide) );
	}

	// Return success
	return true;
}

//! Build the points of the funnel
/*!
 @param pts Receives the points of the funnel, given with their orientation in the funnel
 @param radius The radius of the entity trying to go through the funnel. The path should
               therefore never be closer than radius units of distance from the borders of
			   the funnel (except for the start and end points).
 @param start The starting point of the path
 @param end The ending point of the path
 @param trgl_funnel The triangles the define the channel
 @return True if a path could be found, false otherwise
 */
template <class Vertex>
bool Funnel<Vertex>::build_funnel(std::list< FunnelPoint >& pts, float radius,
								  const Vertex* start, const Vertex* end, const std::vector<Trgl>& trgl_funnel)
{
	// First build the funnel
	if ( build_funnel(pts, start, end, trgl_funnel) == false ) return false;

	// Move right points according to the radius
	{{
		// Get first point
		std::list< FunnelPoint>::iterator it = pts.begin();
		const FunnelPoint* pt0 = &(*it);

		// Get first right point
		while ( it->second != RightSide ) {
			++it;
			if ( it->second == NoSide ) break;
		}

		if ( it->second == RightSide ) {
			const FunnelPoint* pt1 = &(*it);

			// Iterate over the funnel points
			float dx = 0.0f, dy = 0.0f;
			while ( it->second != NoSide ) {
				++it;

				// Nothing to do with left side points
				if ( it->second == LeftSide ) continue;

				// If point is on the right side (or no side), we want to move p1
				float len = sqrt( (pt1->first.x()-pt0->first.x())*(pt1->first.x()-pt0->first.x())
								+ (pt1->first.y()-pt0->first.y())*(pt1->first.y()-pt0->first.y()) );
				float x0 = pt1->first.x() + len*(pt0->first.x()-pt1->first.x());
				float y0 = pt1->first.y() + len*(pt0->first.y()-pt1->first.y());
				pt0->first.x() += dx;
				pt0->first.y() += dy;
				dx = -pt1->first.y() + y0;
				dy =  pt1->first.x() - x0;
				pt0 = pt1;
				pt1 = &(*it);
			}

			// Deal with last point
			pt0->first.x() += dx;
			pt0->first.y() += dy;
		}
	}}

	// Move left points according to the radius
	{{
		// Get first point
		std::list< FunnelPoint>::iterator it = pts.begin();
		const FunnelPoint* pt0 = &(*it);

		// Get first left point
		while ( it->second != LeftSide ) {
			++it;
			if ( it->second == NoSide ) break;
		}

		if ( it->second == LeftSide ) {
			const FunnelPoint* pt1 = &(*it);

			// Iterate over the funnel points
			float dx = 0.0f, dy = 0.0f;
			while ( it->second != NoSide ) {
				++it;

				// Nothing to do with right side points
				if ( it->second == RightSide ) continue;

				// If point is on the left side (or no side), we want to move p1
				float len = sqrt( (pt1->first.x()-pt0->first.x())*(pt1->first.x()-pt0->first.x())
								+ (pt1->first.y()-pt0->first.y())*(pt1->first.y()-pt0->first.y()) );
				float x0 = pt1->first.x() + len*(pt0->first.x()-pt1->first.x());
				float y0 = pt1->first.y() + len*(pt0->first.y()-pt1->first.y());
				pt0->first.x() += dx;
				pt0->first.y() += dy;
				dx =  pt1->first.y() - y0;
				dy = -pt1->first.x() + x0;
				pt0 = pt1;
				pt1 = &(*it);
			}

			// Deal with last point
			pt0->first.x() += dx;
			pt0->first.y() += dy;
		}
	}}

	// Return success
	return true;
}

//! Find the shortest path from a set of funnel points
/*!
 @param path Receives the shortest path in the funnel
 @param pts The points of the funnel, given with their orientation in the funnel
 @param end The ending point of the path
 @return True if a path could be found, false otherwise
 */
template <class Vertex>
bool Funnel<Vertex>::build_path(std::list<Vertex>& path, const std::list< FunnelPoint >& pts, const Vertex* end) {
	// Create funnel and set first triangle
	std::deque< FunnelPoint > funnel;
	{{
		// Fill first two vertices
		std::list< FunnelPoint >::const_iterator it = pts.begin();
		int idx = 0;
		funnel.push_back(*it); ++it; ++idx;
		if ( it->second == RightSide ) funnel.push_back(*it);
		else funnel.push_front(*it);
		++it; ++idx;
		while ( it != pts.end() ) {
			// Get current point
			const FunnelPoint& pt = *it;

			// Pop appropriate points
			if ( pt.second == RightSide ) {
				int itd = (int)funnel.size() - 1; int itd_next = itd - 1;
				while ( itd_next >= 0 ) {
					// If orientation is consistent, nothing to pop
					Side s = ( funnel[itd_next].second == LeftSide ? RightSide : LeftSide );
					if ( side(pt.first, funnel[itd].first, funnel[itd_next].first) != s ) break;

					// If we reached the appex, add it to the path and update new appex
					if ( funnel[itd].second == NoSide ) {
						path.push_back(funnel[itd_next].first);
						funnel[itd_next].second = NoSide;
						funnel.pop_back();
						break;
					}

					// Pop vertex and head to the next (poppable?) vertex
					itd = itd_next; --itd_next;
					funnel.pop_back();
				}

				// Add the point
				funnel.push_back(pt);
			}
			else { // if ( pt.second == LeftSide )
				int itd = 0; int itd_next = 1;
				while ( itd_next < (int)funnel.size() ) {
					// If orientation is consistent, nothing to pop
					Side s = ( funnel[itd_next].second == RightSide ? LeftSide : RightSide );
					if ( side(pt.first, funnel[itd].first, funnel[itd_next].first) != s ) break;

					// If we reached the appex, add it to the path and update new appex
					if ( funnel[itd].second == NoSide ) {
						path.push_back(funnel[itd_next].first);
						funnel[itd_next].second = NoSide;
						funnel.pop_front();
						break;
					}

					// Pop vertex and head to the next (poppable?) vertex
					funnel.pop_front();
				}

				// Add the point
				funnel.push_front(pt);
			}

			// Head to the next point
			++it; ++idx;
		}
	}}
	path.push_back(*end);
	return true;
}

//! Detect the orientation of the three points
/*!
 @param v0 The first point
 @param v1 The second point
 @param v2 The third point
 @return The orientation of the three points
 */
template <class Vertex>
typename Funnel<Vertex>::Side Funnel<Vertex>::side(const Vertex& v0, const Vertex& v1, const Vertex& v2) {
	float tmp = ( (v2.x()-v1.x())*v0.y() + (v1.x()-v0.x())*v2.y() + (v0.x()-v2.x())*v1.y() );
	if ( tmp < 0 ) return RightSide;
	if ( tmp > 0 ) return LeftSide;
	return NoSide;
}

//! Detect the orientation of the funnel triangle
/*!
 @param trgl The funnel triangle
 @return The orientation of the funnel triangle
 */
template <class Vertex>
typename Funnel<Vertex>::Side Funnel<Vertex>::side(const Trgl& trgl) {
	return side(*trgl.v0_, *trgl.v1_, *trgl.v2_);
}

//! Find the vertices that are shared by the two funnel triangles, if any
/*!
 @param vertices Receives the vertices shared by the two triangles
 @param trgl1 The first triangle
 @param trgl2 The second triangle
 @return True if the two triangles share at least two common vertices, false otherwise.
 */
template <class Vertex>
bool Funnel<Vertex>::common_vertices(InternalEdge& vertices, const Trgl& trgl1, const Trgl& trgl2) {
	bool first_found = false;

	// Test first vertex of trgl1
	if ( trgl1.v0_ == trgl2.v0_ ) {
		vertices.first = trgl1.v0_;
		first_found = true;
	}
	else if ( trgl1.v0_ == trgl2.v1_ ) {
		vertices.first = trgl1.v0_;
		first_found = true;
	}
	else if ( trgl1.v0_ == trgl2.v2_ ) {
		vertices.first = trgl1.v0_;
		first_found = true;
	}

	// Test second vertex of trgl1
	if ( trgl1.v1_ == trgl2.v0_ ) {
		if ( first_found == true ) {
			vertices.second = trgl1.v1_;
			return true;
		}
		vertices.first = trgl1.v1_;
		first_found = true;
	}
	else if ( trgl1.v1_ == trgl2.v1_ ) {
		if ( first_found == true ) {
			vertices.second = trgl1.v1_;
			return true;
		}
		vertices.first = trgl1.v1_;
		first_found = true;
	}
	else if ( trgl1.v1_ == trgl2.v2_ ) {
		if ( first_found == true ) {
			vertices.second = trgl1.v1_;
			return true;
		}
		vertices.first = trgl1.v1_;
		first_found = true;
	}

	// If a first common vertex has not yet been found, there is no hope to find TWO common vertices
	if ( first_found == false ) return false;

	// Test third vertex of trgl1
	if ( trgl1.v2_ == trgl2.v0_ ) {
		vertices.second = trgl1.v2_;
		return true;
	}
	else if ( trgl1.v2_ == trgl2.v1_ ) {
		vertices.second = trgl1.v2_;
		return true;
	}
	else if ( trgl1.v2_ == trgl2.v2_ ) {
		vertices.second = trgl1.v2_;
		return true;
	}
	return false;
}

//! Detect which vertex, in the first triangle, is not shared with the edge
/*!
 @param trgl The funnel triangle to test
 @param edge The edge of a funnel triangle, to which the returned vertex must NOT belong
 @return A pointer to the non-shared vertex (may be nil if there is no such vertex)
 */
template <class Vertex>
const Vertex* Funnel<Vertex>::opposite_vertex(const Trgl& trgl, const InternalEdge& edge) {
	// Get the last vertex
	if ( edge.first == trgl.v0_ ) {
		if ( edge.second == trgl.v1_ ) return trgl.v2_;
		return trgl.v1_;
	}
	if ( edge.first == trgl.v1_ ) {
		if ( edge.second == trgl.v0_ ) return trgl.v2_;
		return trgl.v0_;
	}
	if ( edge.first == trgl.v2_ ) {
		if ( edge.second == trgl.v0_ ) return trgl.v1_;
		return trgl.v0_;
	}
	return nil;
}

//! Detect which vertex, in the first triangle, is not shared with the other triangle
/*!
 @param trgl The first funnel triangle to test. Returned vertex will belong to this triangle.
 @param trgl The other funnel triangle to test. Returned vertex will NOT belong to this triangle.
 @return A pointer to the non-shared vertex (may be nil if there is no such vertex)
 */
template <class Vertex>
const Vertex* Funnel<Vertex>::opposite_vertex(const Trgl& trgl, const Trgl& other_trgl) {
	// Get common vertices
	InternalEdge edge;
	if ( common_vertices(edge, trgl, other_trgl) == false ) return nil;
	return opposite_vertex(trgl, edge);
}

//=================================================================================

#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif

#endif
